#include "JointUtils.h"

#include <cmath>
#include <assert.h>

#ifndef RSIM_SMALL
	#define RSIM_SMALL 1E-10
#endif	

using namespace RSIM;

void JointUtils::calc_cFk(const double *F, const double *I, const double *A, double *p) const{
	p[0]=F[0]-A[0]*I[0]-A[1]*I[1]-A[2]*I[2]-A[3]*I[3]-A[4]*I[4]-A[5]*I[5];
	p[1]=F[1]-A[4]*I[10]-A[0]*I[6]-A[5]*I[11]-A[1]*I[7]-A[2]*I[8]-A[3]*I[9];
	p[2]=F[2]-A[0]*I[12]-A[1]*I[13]-A[2]*I[14]-A[3]*I[15]-A[4]*I[16]-A[5]*I[17];
	p[3]=F[3]-A[0]*I[18]-A[1]*I[19]-A[2]*I[20]-A[3]*I[21]-A[4]*I[22]-A[5]*I[23];
	p[4]=F[4]-A[0]*I[24]-A[1]*I[25]-A[2]*I[26]-A[3]*I[27]-A[4]*I[28]-A[5]*I[29];
	p[5]=F[5]-A[0]*I[30]-A[1]*I[31]-A[2]*I[32]-A[3]*I[33]-A[4]*I[34]-A[5]*I[35];	
}

void JointUtils::calc_m_cFk(const double *F, const double *I, const double *A, double *p) const{
	p[0]=-(F[0]-A[0]*I[0]-A[1]*I[1]-A[2]*I[2]-A[3]*I[3]-A[4]*I[4]-A[5]*I[5]);
	p[1]=-(F[1]-A[4]*I[10]-A[0]*I[6]-A[5]*I[11]-A[1]*I[7]-A[2]*I[8]-A[3]*I[9]);
	p[2]=-(F[2]-A[0]*I[12]-A[1]*I[13]-A[2]*I[14]-A[3]*I[15]-A[4]*I[16]-A[5]*I[17]);
	p[3]=-(F[3]-A[0]*I[18]-A[1]*I[19]-A[2]*I[20]-A[3]*I[21]-A[4]*I[22]-A[5]*I[23]);
	p[4]=-(F[4]-A[0]*I[24]-A[1]*I[25]-A[2]*I[26]-A[3]*I[27]-A[4]*I[28]-A[5]*I[29]);
	p[5]=-(F[5]-A[0]*I[30]-A[1]*I[31]-A[2]*I[32]-A[3]*I[33]-A[4]*I[34]-A[5]*I[35]);
}

void JointUtils::calc_skbeta_x_Ik3_x_tskbeta(const double *I, const double *beta, double *p)const{
	double t[29];
	
	t[2]=I[35]*beta[1];
	t[10]=I[29]*beta[2];
	t[3]=I[5]-t[10]+t[2];
	t[4]=I[33]*beta[1];
	t[8]=I[27]*beta[2];
	t[5]=I[3]+t[4]-t[8];
	t[6]=I[34]*beta[1];
	t[9]=I[28]*beta[2];
	t[7]=I[4]+t[6]-t[9];
	t[11]=I[23]*beta[2];
	t[19]=I[35]*beta[0];
	t[12]=I[11]+t[11]-t[19];
	t[13]=I[22]*beta[2];
	t[18]=I[34]*beta[0];
	t[14]=I[10]+t[13]-t[18];
	t[15]=I[21]*beta[2];
	t[17]=I[33]*beta[0];
	t[16]=I[9]+t[15]-t[17];
	t[20]=I[29]*beta[0];
	t[28]=I[23]*beta[1];
	t[21]=I[17]+t[20]-t[28];
	t[22]=I[27]*beta[0];
	t[26]=I[21]*beta[1];
	t[23]=I[15]+t[22]-t[26];
	t[24]=I[28]*beta[0];
	t[27]=I[22]*beta[1];
	t[25]=I[16]+t[24]-t[27];
	
	p[0]=I[0]-I[24]*beta[2]+I[30]*beta[1]+beta[1]*t[3]-beta[2]*t[7];
	p[1]=I[1]-I[25]*beta[2]+I[31]*beta[1]-beta[0]*t[3]+beta[2]*t[5];
	p[2]=I[2]-I[26]*beta[2]+I[32]*beta[1]-beta[1]*t[5]+beta[0]*t[7];
	p[3]=t[5];
	p[4]=t[7];
	p[5]=t[3];
	p[6]=I[6]+I[18]*beta[2]-I[30]*beta[0]+beta[1]*t[12]-beta[2]*t[14];
	p[7]=I[7]+I[19]*beta[2]-I[31]*beta[0]-beta[0]*t[12]+beta[2]*t[16];
	p[8]=I[8]+I[20]*beta[2]-I[32]*beta[0]+beta[0]*t[14]-beta[1]*t[16];
	p[9]=t[16];
	p[10]=t[14];
	p[11]=t[12];
	p[12]=I[12]-I[18]*beta[1]+I[24]*beta[0]+beta[1]*t[21]-beta[2]*t[25];
	p[13]=I[13]-I[19]*beta[1]+I[25]*beta[0]-beta[0]*t[21]+beta[2]*t[23];
	p[14]=I[14]-I[20]*beta[1]+I[26]*beta[0]-beta[1]*t[23]+beta[0]*t[25];
	p[15]=t[23];
	p[16]=t[25];
	p[17]=t[21];
	p[18]=I[18]-t[13]+t[28];
	p[19]=I[19]+t[15]-I[23]*beta[0];
	p[20]=I[20]-t[26]+I[22]*beta[0];
	p[21]=I[21];
	p[22]=I[22];
	p[23]=I[23];
	p[24]=I[24]-t[9]+I[29]*beta[1];
	p[25]=I[25]-t[20]+t[8];
	p[26]=I[26]+t[24]-I[27]*beta[1];
	p[27]=I[27];
	p[28]=I[28];
	p[29]=I[29];
	p[30]=I[30]+t[2]-I[34]*beta[2];
	p[31]=I[31]-t[19]+I[33]*beta[2];
	p[32]=I[32]+t[18]-t[4];
	p[33]=I[33];
	p[34]=I[34];
	p[35]=I[35];
}

void JointUtils::calc_skbeta_x_cFk(const double *F, const double *beta, double *p)const{
	 p[0 ]= F[0] - F[4]*beta[2] + F[5]*beta[1];
	 p[1] = F[1] + F[3]*beta[2] - F[5]*beta[0];
	 p[2] = F[2] - F[3]*beta[1] + F[4]*beta[0];
         p[3] = F[3];
	 p[4] = F[4];
	 p[5] = F[5];
}

///////////////////////////////////////////////////////

void JointUtils::calc_Triang_x_Ik3_RotZ(const double *I, double *p)const{
	
	#ifdef RSIM_SAFE
	assert(fabs(I[14])>RSIM_SMALL);
	#endif
	
	double Z = 1.0/I[14];
	
	p[0]=I[0]-I[12]*I[2]*Z;
	p[1]=I[1]-I[13]*I[2]*Z;
	p[2]=0.0;
	p[3]=I[3]-I[15]*I[2]*Z;
	p[4]=I[4]-I[16]*I[2]*Z;
	p[5]=I[5]-I[17]*I[2]*Z;
	p[6]=I[6]-I[12]*I[8]*Z;
	p[7]=I[7]-I[13]*I[8]*Z;
	p[8]=0.0;
	p[9]=I[9]-I[15]*I[8]*Z;
	p[10]=I[10]-I[16]*I[8]*Z;
	p[11]=I[11]-I[17]*I[8]*Z;
	p[12]=0.0;
	p[13]=0.0;
	p[14]=0.0;
	p[15]=0.0;
	p[16]=0.0;
	p[17]=0.0;
	p[18]=I[18]-I[12]*I[20]*Z;
	p[19]=I[19]-I[13]*I[20]*Z;
	p[20]=0.0;
	p[21]=I[21]-I[15]*I[20]*Z;
	p[22]=I[22]-I[16]*I[20]*Z;
	p[23]=I[23]-I[17]*I[20]*Z;
	p[24]=I[24]-I[12]*I[26]*Z;
	p[25]=I[25]-I[13]*I[26]*Z;
	p[26]=0.0;
	p[27]=I[27]-I[15]*I[26]*Z;
	p[28]=I[28]-I[16]*I[26]*Z;
	p[29]=I[29]-I[17]*I[26]*Z;
	p[30]=I[30]-I[12]*I[32]*Z;
	p[31]=I[31]-I[13]*I[32]*Z;
	p[32]=0.0;
	p[33]=I[33]-I[15]*I[32]*Z;
	p[34]=I[34]-I[16]*I[32]*Z;
	p[35]=I[35]-I[17]*I[32]*Z;
}

////////////////////////////////////////////////////////////////////////

void JointUtils::calc_Triang_x_cFk_RotZ(const double *I, const double *F, double *p)const{
	
	#ifdef RSIM_SAFE
	assert(fabs(I[14])>RSIM_SMALL);
	#endif
	
	double Z = 1.0/I[14];
	 
	p[0]=F[0]-(F[2]*I[2])*Z;
	p[1]=F[1]-(F[2]*I[8])*Z;
	p[2]=0.0;
	p[3]=F[3]-(F[2]*I[20])*Z;
	p[4]=F[4]-(F[2]*I[26])*Z;
	p[5]=F[5]-(F[2]*I[32])*Z;
}

////////////////////////////////////////////////////////////////////////

void JointUtils::calc_Triang_x_Ik3_TransZ(const double *I, double *p)const{
	
	#ifdef RSIM_SAFE
	assert(fabs(I[35])>RSIM_SMALL);
	#endif
	
	double Z = 1.0/I[35];
	
	p[0]=I[0]-I[30]*I[5]*Z;
	p[1]=I[1]-I[31]*I[5]*Z;
	p[2]=I[2]-I[32]*I[5]*Z;
	p[3]=I[3]-I[33]*I[5]*Z;
	p[4]=I[4]-I[34]*I[5]*Z;
	p[5]=0.0;
	p[6]=I[6]-I[11]*I[30]*Z;
	p[7]=I[7]-I[11]*I[31]*Z;
	p[8]=I[8]-I[11]*I[32]*Z;
	p[9]=I[9]-I[11]*I[33]*Z;
	p[10]=I[10]-I[11]*I[34]*Z;
	p[11]=0.0;
	p[12]=I[12]-I[17]*I[30]*Z;
	p[13]=I[13]-I[17]*I[31]*Z;
	p[14]=I[14]-I[17]*I[32]*Z;
	p[15]=I[15]-I[17]*I[33]*Z;
	p[16]=I[16]-I[17]*I[34]*Z;
	p[17]=0.0;
	p[18]=I[18]-I[23]*I[30]*Z;
	p[19]=I[19]-I[23]*I[31]*Z;
	p[20]=I[20]-I[23]*I[32]*Z;
	p[21]=I[21]-I[23]*I[33]*Z;
	p[22]=I[22]-I[23]*I[34]*Z;
	p[23]=0.0;
	p[24]=I[24]-I[29]*I[30]*Z;
	p[25]=I[25]-I[29]*I[31]*Z;
	p[26]=I[26]-I[29]*I[32]*Z;
	p[27]=I[27]-I[29]*I[33]*Z;
	p[28]=I[28]-I[29]*I[34]*Z;
	p[29]=0.0;
	p[30]=0.0;
	p[31]=0.0;
	p[32]=0.0;
	p[33]=0.0;
	p[34]=0.0;
	p[35]=0.0;
}

////////////////////////////////////////////////////////////////////////

void JointUtils::calc_Triang_x_cFk_TransZ(const double *I, const double *F, double *p)const{
	
	#ifdef RSIM_SAFE
	assert(fabs(I[35])>RSIM_SMALL);
	#endif
	
	double Z = 1.0/I[35];
	
	p[0] = F[0]-(F[5]*I[5])*Z;
	p[1] = F[1]-(F[5]*I[11])*Z;
	p[2] = F[2]-(F[5]*I[17])*Z;
	p[3] = F[3]-(F[5]*I[23])*Z;
	p[4] = F[4]-(F[5]*I[29])*Z;
	p[5] = 0.0;
}

////////////////////////////////////////////////////////////////////////

void JointUtils::calc_Triang_x_Ik3_TransX(const double *I, double *p)const{
	double Z;
	
	#ifdef RSIM_SAFE
	assert(fabs(I[21])>RSIM_SMALL);
	#endif
	
	Z=1.0/I[21];
	
	p[0]=I[0]-I[18]*I[3]*Z;
	p[1]=I[1]-I[19]*I[3]*Z;
	p[2]=I[2]-I[20]*I[3]*Z;
	p[3]=0.0;
	p[4]=I[4]-I[22]*I[3]*Z;
	p[5]=I[5]-I[23]*I[3]*Z;
	p[6]=I[6]-I[18]*I[9]*Z;
	p[7]=I[7]-I[19]*I[9]*Z;
	p[8]=I[8]-I[20]*I[9]*Z;
	p[9]=0.0;
	p[10]=I[10]-I[22]*I[9]*Z;
	p[11]=I[11]-I[23]*I[9]*Z;
	p[12]=I[12]-I[15]*I[18]*Z;
	p[13]=I[13]-I[15]*I[19]*Z;
	p[14]=I[14]-I[15]*I[20]*Z;
	p[15]=0.0;
	p[16]=I[16]-I[15]*I[22]*Z;
	p[17]=I[17]-I[15]*I[23]*Z;
	p[18]=0.0;
	p[19]=0.0;
	p[20]=0.0;
	p[21]=0.0;
	p[22]=0.0;
	p[23]=0.0;	
	p[24]=I[24]-I[18]*I[27]*Z;
	p[25]=I[25]-I[19]*I[27]*Z;
	p[26]=I[26]-I[20]*I[27]*Z;
	p[27]=0.0;
	p[28]=I[28]-I[22]*I[27]*Z;
	p[29]=I[29]-I[23]*I[27]*Z;
	p[30]=I[30]-I[18]*I[33]*Z;
	p[31]=I[31]-I[19]*I[33]*Z;
	p[32]=I[32]-I[20]*I[33]*Z;
	p[33]=0.0;
	p[34]=I[34]-I[22]*I[33]*Z;
	p[35]=I[35]-I[23]*I[33]*Z;
}

void JointUtils::calc_Triang_x_cFk_TransX(const double *I, const double *F, double *p)const{
	double Z;
	
	#ifdef RSIM_SAFE
	assert(fabs(I[21])>RSIM_SMALL);
	#endif
	
	Z=1.0/I[21];
	
	p[0]=F[0]-F[3]*I[3]*Z;
	p[1]=F[1]-F[3]*I[9]*Z;
	p[2]=F[2]-F[3]*I[15]*Z;
	p[3]=0.0;
	p[4]=F[4]-F[3]*I[27]*Z;
	p[5]=F[5]-F[3]*I[33]*Z;
}

void JointUtils::calc_Triang_x_Ik3_TransY(const double *I, double *p)const{
	double Z;
	
	#ifdef RSIM_SAFE
	assert(fabs(I[28])>RSIM_SMALL);
	#endif
	
	Z=1.0/I[28];
	p[0]=I[0]-I[24]*I[4]*Z;
	p[1]=I[1]-I[25]*I[4]*Z;
	p[2]=I[2]-I[26]*I[4]*Z;
	p[3]=I[3]-I[27]*I[4]*Z;
	p[4]=0.0;
	p[5]=I[5]-I[29]*I[4]*Z;
	p[6]=I[6]-I[10]*I[24]*Z;
	p[7]=I[7]-I[10]*I[25]*Z;
	p[8]=I[8]-I[10]*I[26]*Z;
	p[9]=I[9]-I[10]*I[27]*Z;
	p[10]=0.0;
	p[11]=I[11]-I[10]*I[29]*Z;
	p[12]=I[12]-I[16]*I[24]*Z;
	p[13]=I[13]-I[16]*I[25]*Z;
	p[14]=I[14]-I[16]*I[26]*Z;
	p[15]=I[15]-I[16]*I[27]*Z;
	p[16]=0.0;
	p[17]=I[17]-I[16]*I[29]*Z;
	p[18]=I[18]-I[22]*I[24]*Z;
	p[19]=I[19]-I[22]*I[25]*Z;
	p[20]=I[20]-I[22]*I[26]*Z;
	p[21]=I[21]-I[22]*I[27]*Z;
	p[22]=0.0;
	p[23]=I[23]-I[22]*I[29]*Z;
	p[24]=0.0;
	p[25]=0.0;
	p[26]=0.0;
	p[27]=0.0;
	p[28]=0.0;
	p[29]=0.0;
	p[30]=I[30]-I[24]*I[34]*Z;
	p[31]=I[31]-I[25]*I[34]*Z;
	p[32]=I[32]-I[26]*I[34]*Z;
	p[33]=I[33]-I[27]*I[34]*Z;
	p[34]=0.0;
	p[35]=I[35]-I[29]*I[34]*Z;
}

void JointUtils::calc_Triang_x_cFk_TransY(const double *I, const double *F, double *p)const{
	double Z;
	
	#ifdef RSIM_SAFE
	assert(fabs(I[28])>RSIM_SMALL);
	#endif
	
	Z=1.0/I[28];
	p[0]=F[0]-F[4]*I[4]*Z;
	p[1]=F[1]-F[4]*I[10]*Z;
	p[2]=F[2]-F[4]*I[16]*Z;
	p[3]=F[3]-F[4]*I[22]*Z;
	p[4]=0.0;
	p[5]=F[5]-F[4]*I[34]*Z;
}

void JointUtils::calc_Triang_x_Ik3_RotX(const double *I, double *p)const{
	double Z;
	
	#ifdef RSIM_SAFE
	assert(fabs(I[0])>RSIM_SMALL);
	#endif
	
	Z=1.0/I[0];
	p[0]=0.0;
	p[1]=0.0;
	p[2]=0.0;
	p[3]=0.0;
	p[4]=0.0;
	p[5]=0.0;
	p[6]=0.0;
	p[7]=I[7]-I[1]*I[6]*Z;
	p[8]=I[8]-I[2]*I[6]*Z;
	p[9]=I[9]-I[3]*I[6]*Z;
	p[10]=I[10]-I[4]*I[6]*Z;
	p[11]=I[11]-I[5]*I[6]*Z;
	p[12]=0.0;
	p[13]=I[13]-I[1]*I[12]*Z;
	p[14]=I[14]-I[12]*I[2]*Z;
	p[15]=I[15]-I[12]*I[3]*Z;
	p[16]=I[16]-I[12]*I[4]*Z;
	p[17]=I[17]-I[12]*I[5]*Z;
	p[18]=0.0;
	p[19]=I[19]-I[1]*I[18]*Z;
	p[20]=I[20]-I[18]*I[2]*Z;
	p[21]=I[21]-I[18]*I[3]*Z;
	p[22]=I[22]-I[18]*I[4]*Z;
	p[23]=I[23]-I[18]*I[5]*Z;
	p[24]=0.0;
	p[25]=I[25]-I[1]*I[24]*Z;
	p[26]=I[26]-I[2]*I[24]*Z;
	p[27]=I[27]-I[24]*I[3]*Z;
	p[28]=I[28]-I[24]*I[4]*Z;
	p[29]=I[29]-I[24]*I[5]*Z;
	p[30]=0.0;
	p[31]=I[31]-I[1]*I[30]*Z;
	p[32]=I[32]-I[2]*I[30]*Z;
	p[33]=I[33]-I[3]*I[30]*Z;
	p[34]=I[34]-I[30]*I[4]*Z;
	p[35]=I[35]-I[30]*I[5]*Z;	
}

void JointUtils::calc_Triang_x_cFk_RotX(const double *I, const double *F, double *p)const{
	double Z;
	
	#ifdef RSIM_SAFE
	assert(fabs(I[0])>RSIM_SMALL);
	#endif
	
	Z=1.0/I[0];
	p[0]=0.0;
	p[1]=F[1]-F[0]*I[6]*Z;
	p[2]=F[2]-F[0]*I[12]*Z;
	p[3]=F[3]-F[0]*I[18]*Z;
	p[4]=F[4]-F[0]*I[24]*Z;
	p[5]=F[5]-F[0]*I[30]*Z;
}

void JointUtils::calc_Triang_x_Ik3_RotY(const double *I, double *p)const{	
	double Z;
	
	#ifdef RSIM_SAFE
	assert(fabs(I[7])>RSIM_SMALL);
	#endif
	
	Z=1.0/I[7];
	
	p[0]=I[0]-I[1]*I[6]*Z;
	p[1]=0.0;
	p[2]=I[2]-I[1]*I[8]*Z;
	p[3]=I[3]-I[1]*I[9]*Z;
	p[4]=I[4]-I[1]*I[10]*Z;
	p[5]=I[5]-I[1]*I[11]*Z;
	p[6]=0.0;
	p[7]=0.0;
	p[8]=0.0;
	p[9]=0.0;
	p[10]=0.0;
	p[11]=0.0;
	p[12]=I[12]-I[13]*I[6]*Z;
	p[13]=0.0;
	p[14]=I[14]-I[13]*I[8]*Z;
	p[15]=I[15]-I[13]*I[9]*Z;
	p[16]=I[16]-I[10]*I[13]*Z;
	p[17]=I[17]-I[11]*I[13]*Z;
	p[18]=I[18]-I[19]*I[6]*Z;
	p[19]=0.0;
	p[20]=I[20]-I[19]*I[8]*Z;
	p[21]=I[21]-I[19]*I[9]*Z;
	p[22]=I[22]-I[10]*I[19]*Z;
	p[23]=I[23]-I[11]*I[19]*Z;
	p[24]=I[24]-I[25]*I[6]*Z;
	p[25]=0.0;
	p[26]=I[26]-I[25]*I[8]*Z;
	p[27]=I[27]-I[25]*I[9]*Z;
	p[28]=I[28]-I[10]*I[25]*Z;
	p[29]=I[29]-I[11]*I[25]*Z;
	p[30]=I[30]-I[31]*I[6]*Z;
	p[31]=0.0;
	p[32]=I[32]-I[31]*I[8]*Z;
	p[33]=I[33]-I[31]*I[9]*Z;
	p[34]=I[34]-I[10]*I[31]*Z;
	p[35]=I[35]-I[11]*I[31]*Z;
}


void JointUtils::calc_Triang_x_cFk_RotY(const double *I, const double *F, double *p)const{
	double Z;
	
	#ifdef RSIM_SAFE
	assert(fabs(I[7])>RSIM_SMALL);
	#endif
	
	Z=1.0/I[7];
	
	p[0]=F[0]-F[1]*I[1]*Z;
	p[1]=0.0;
	p[2]=F[2]-F[1]*I[13]*Z;
	p[3]=F[3]-F[1]*I[19]*Z;
	p[4]=F[4]-F[1]*I[25]*Z;
	p[5]=F[5]-F[1]*I[31]*Z;
}